#include "odometry.h"
// Gydata Gyodometry::Tdata;
// float Gyodometry::sectime0;
// // double Gyodometry::sectime1
// std::mutex Gyodometry::data_mu1;
// hitcrt::SerialApp::RECEIVE_FLAG Gyodometry::flag;

/*
    数据获取
    开一单独的线程进行陀螺仪数据的获取并实时更新。
*/
void Gyodometry::dataCL()
{
    hitcrt::SerialApp serial("auto", 115200);
    struct timeval time_t;
    gettimeofday(&time_t, NULL);
    m_sectime0 = time_t.tv_sec + time_t.tv_usec * 1e-6;
    std::vector<float> receive_data;
    // std::cout << "receive_data: " << receive_data[2] << ' ' << receive_data[3] << std::endl;
    double t0, t1;
    std::cout << "init " << std::endl;
    m_Tdata.Tran = Eigen::Vector3f::Zero();
    m_Tdata.yaw = 0.;
    m_Tdata.sectime = 0.0;
    //循环获取数据
    while (true)
    {
        // receive_data.resize(5);
        // receive_data.clear();
        // for(size_t i = 0; i < 5; i++)
        //     receive_data[i] = .0;

        struct timeval time0;
        gettimeofday(&time0, NULL);
        t0 = time0.tv_sec * 1e3 + time0.tv_usec * 1e-3;
        if (!serial.receive(m_flag, receive_data))
        {
            static int count = 0;
            std::cout << "收数据失败" << std::endl;
            if (count % 1000 == 0)
            {
                m_Tdata.Tran = Eigen::Vector3f::Zero();
                m_Tdata.yaw = 0;
                m_Tdata.sectime = 0.0;
            }
        }
        else
        {
            // std::cout << "收数据成功" << std::endl;
            // for(size_t i = 0; i < receive_data.size(); i++)
            //     std::cout << receive_data[i] << "  ";
            // std::cout << std::endl;
            m_data_mu1.lock();
            gettimeofday(&time0, NULL);
            // std::cout << "receive: " << receive_data[0] << ' ' << receive_data[1] << ' ' << receive_data[2] << ' ' << receive_data[3] << ' ' << receive_data[4] << std::endl;
            // Tdata.yaw = receive_data[2] - receive_data[1] ;
            m_Tdata.yaw = receive_data[2];
            // std::cout << "yaw: " << Tdata.yaw << std::endl;
            m_Tdata.Tran << receive_data[3] / 1000., receive_data[4] / 1000., 0;
            // std::cout << Tdata.Tran << std::endl;
            m_Tdata.sectime = time0.tv_sec + time0.tv_usec * 1e-6;
            m_data_mu1.unlock();
        }
        gettimeofday(&time0, NULL);
        t1 = time0.tv_sec * 1e3 + time0.tv_usec * 1e-3;
        // std::cout << "接收的FPS为: " << 1000 / (t1 - t0) << std::endl;
    }
}

/*
    单次返回数据，用于点云配准
*/
void Gyodometry::dataCB(Gydata& inputdata)
{
    // Gydata Dtemp;
    m_data_mu1.lock();
    inputdata.sectime = m_Tdata.sectime;
    inputdata.Tran = m_Tdata.Tran;
    inputdata.yaw = m_Tdata.yaw;
    // std::cout << "DataCB is: " << inputdata.yaw << std::endl;
    m_data_mu1.unlock();
    // return Dtemp;
}
